/**
 * @author Jerome Monceaux
 * @version 1.0
 * 
 * GenericVirtualMotor : This class allow differential drive motor simulation.
 **/
package org.hh.locodo;

import org.hh.locodo.MotorInterface;

import ioio.lib.api.DigitalOutput;
import ioio.lib.api.IOIO;
import ioio.lib.api.PwmOutput;
import ioio.lib.api.DigitalOutput.Spec.Mode;
import ioio.lib.api.exception.ConnectionLostException;



public class IoioMotorControlerWithoutEncoder implements MotorInterface {

  private double fDesiredSpeed;
  private double fSpeed;
  private double fSpeedSimul;
  private double fEncVal;
  private double fMaxAcc;
	private int fPin;
	private IOIO fIoio;
	
	private PwmOutput fPwmOutput;

	
  @SuppressWarnings("unused")
	private double fMaxSpeed;


  
	public IoioMotorControlerWithoutEncoder(int pin, IOIO ioio) throws ConnectionLostException {
	  fPin = pin;
		fSpeedSimul = 0.f;
	  fEncVal = 0.f;
	  fMaxAcc = 0.1f; // encoder ticks per cycle
	  fDesiredSpeed = 0.f;
	  fMaxSpeed = 0.5f;
	  fIoio = ioio;
	  
		fPwmOutput = fIoio.openPwmOutput(new DigitalOutput.Spec(fPin, Mode.OPEN_DRAIN), 100);
		fPwmOutput = fIoio.openPwmOutput(new DigitalOutput.Spec(fPin, Mode.OPEN_DRAIN), 100);
	}

	@Override
	public int getEncoderValue() {
		return (int) fEncVal;
	}

	@Override
	public void setSpeed(double speed) {
		fDesiredSpeed = speed;
	}

	@Override
	public void update() throws ConnectionLostException{
	  if( Math.abs(fDesiredSpeed - fSpeedSimul) < fMaxAcc )
	  	fSpeedSimul = fDesiredSpeed;
	  else
	    if( fDesiredSpeed > fSpeedSimul )
	    	fSpeedSimul += fMaxAcc;
	    else
	    	fSpeedSimul -= fMaxAcc;
	  
	  // Warning do not simulate saturation speed like this
	  // The angle control is not effective at max speed...
	  //if( fSpeed > fMaxSpeed ) fSpeed = fMaxSpeed; 
	  //if( fSpeed < -fMaxSpeed ) fSpeed = -fMaxSpeed; 
	  fEncVal += 200.0 * fSpeedSimul;
	  
	  if( fDesiredSpeed != fSpeed ){
	  	fPwmOutput.setDutyCycle((float) fSpeed);
		  fSpeed = fDesiredSpeed;
	  }
	}

}
